import pyrealsense2 as rs
import numpy as np
import json
import os

# 获取连接的所有相机设备
context = rs.context()
devices = context.query_devices()

# 打印连接的设备数
print(f"Found {len(devices)} devices")

# 存储每个相机的pipeline
pipelines = []

# 遍历所有相机
for i, device in enumerate(devices):
    # 打印相机的名称
    print(f"Opening device {i}: {device.get_info(rs.camera_info.name)}")
    
    # 创建一个Pipeline
    pipeline = rs.pipeline()
    
    # 配置相机流
    config = rs.config()
    config.enable_device(device.get_info(rs.camera_info.serial_number))  # 使用设备序列号来识别相机
    config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)  # 配置颜色流（你可以根据需求更改流类型）
    # 开始流
    profile = pipeline.start(config)

    # 获取RGB相机内参
    color_stream = profile.get_stream(rs.stream.color)
    intr = color_stream.as_video_stream_profile().get_intrinsics()

    # 相机内参矩阵 K
    K = np.array([
        [intr.fx,     0,       intr.ppx],
        [0,       intr.fy,     intr.ppy],
        [0,           0,            1   ]
    ])
    D = intr.coeffs
    print("Camera Intrinsic Matrix K:\n", K)
    print("Distortion Coeffs D:", intr.coeffs)
    pipelines.append({
        'name': device.get_info(rs.camera_info.serial_number),
        'intrinsic': K.tolist(),
        'distortion': D,
    })

with open("calibration\\intrinsics.json", "w") as f:
    json.dump(pipelines, f, indent=4)



